#include "feature/feature_database.h"

namespace feature {

void FeatureDatabase::UpdateFeatureDatabase(const uint8_t& camera_id, const uint64_t& frame_id, const uint64_t& feature_id, const FeaturePoint& feature) {
	if (feature_database_.find(feature_id) != feature_database_.end()) {
    	// Get feature
    	std::shared_ptr<Feature> feat_ptr = feature_database_.at(feature_id);
    	feat_ptr->feature_id_ = feature_id;
    	feat_ptr->map_camera_id_and_feature_[camera_id].push_back(feature);
    	feat_ptr->map_camera_and_frame_id_[camera_id].push_back(frame_id);
    	feature_database_[feature_id] = feat_ptr;
  	} else {
  		//  we have not found the feature, so lets make it be a new one! Append this new feature into our database
  		std::shared_ptr<Feature> feat_ptr = std::make_shared<Feature>();
    	feat_ptr->feature_id_ = feature_id;
    	feat_ptr->map_camera_id_and_feature_[camera_id].push_back(feature);
    	feat_ptr->map_camera_and_frame_id_[camera_id].push_back(frame_id);
  		feature_database_[feature_id] = feat_ptr;
  	}
}

bool FeatureDatabase::GetFeaturePoint(FeaturePoint& feature_point, const uint8_t& camera_id, const uint64_t& frame_id, const uint64_t& feature_id) {
	bool is_find = false;
	const auto iter_database = feature_database_.find(feature_id);
	if (iter_database != feature_database_.end()) {
		const auto iter_frame_ids = iter_database->second->map_camera_and_frame_id_.find(camera_id);
		if(iter_frame_ids != iter_database->second->map_camera_and_frame_id_.end()) {
			const auto iter_frame_id = std::find(iter_frame_ids->second.begin(), iter_frame_ids->second.end(), frame_id);
			if(iter_frame_id != iter_frame_ids->second.end()) {
				uint32_t idx = std::distance(iter_frame_ids->second.begin(), iter_frame_id);
				is_find = true;
				feature_point = iter_database->second->map_camera_id_and_feature_.at(camera_id)[idx];
			}
		}
		
	}

	return is_find;
}

std::map<uint64_t, FeaturePoint> FeatureDatabase::GetOneFrameFeaturePoints(const uint8_t& camera_id, const uint64_t& frame_id) {
	std::map<uint64_t, FeaturePoint> points;
	for(const auto& features : feature_database_) {
		std::shared_ptr<Feature> feat_ptr = features.second;

		// LOG(INFO) << " features id ===== : " << features.first << std::endl;

		for(const auto& map_camera_and_frame_id : feat_ptr->map_camera_and_frame_id_) {
			if(camera_id != map_camera_and_frame_id.first) {
				continue;
			}
			std::vector<uint64_t> frame_ids = map_camera_and_frame_id.second;
			const auto iter_frame_id = std::find(frame_ids.begin(), frame_ids.end(), frame_id);
			if(iter_frame_id != frame_ids.end()) {
				uint32_t idx = std::distance(frame_ids.begin(), iter_frame_id);
				points[feat_ptr->feature_id_] = feat_ptr->map_camera_id_and_feature_.at(camera_id)[idx];
			}
		}
	}
	return points;
}

uint32_t FeatureDatabase::Size() {
	return feature_database_.size();
}

std::unordered_map<uint64_t, std::shared_ptr<Feature>> FeatureDatabase::GetFeatureDatabase() {
	return feature_database_;
}

void FeatureDatabase::UpdateLandmark(const uint64_t& feature_id, const std::shared_ptr<Feature>& feature, const Vector3d& world_point) {
	if (feature_database_.find(feature_id) != feature_database_.end()) {
    	// Get feature
    	std::shared_ptr<Feature> feat_ptr = feature_database_.at(feature_id);
    	feat_ptr->world_point_ = world_point;
    	feature_database_[feature_id] = feat_ptr;
  	}  
}

void FeatureDatabase::EraseLandmarkFromFrameId(const uint8_t& camera_id, const uint64_t& frame_id) {
	for(auto& features : feature_database_) {
		std::shared_ptr<Feature>& feat_ptr = features.second;

		// LOG(INFO) << " features id ===== : " << features.first << std::endl;

		for(auto& map_camera_and_frame_id : feat_ptr->map_camera_and_frame_id_) {
			if(camera_id != map_camera_and_frame_id.first) {
				continue;
			}
			std::vector<uint64_t>& frame_ids = map_camera_and_frame_id.second;
			if(frame_ids.empty()) {
				continue;
			}
			
			LOG(INFO) << " frame_ids 0===== : " << frame_ids.size() << std::endl;
			// LOG(INFO) << " feat_ptr->map_camera_id_and_feature_.at(camera_id) 0===== : " << feat_ptr->map_camera_id_and_feature_.at(camera_id).size() << std::endl;

			const auto iter_frame_id = std::find(frame_ids.begin(), frame_ids.end(), frame_id);
			if(iter_frame_id != frame_ids.end()) {
				uint32_t idx = std::distance(frame_ids.begin(), iter_frame_id);

				// LOG(INFO) << " idx===== : " << idx << std::endl;

				feat_ptr->map_camera_id_and_feature_.at(camera_id).erase(feat_ptr->map_camera_id_and_feature_.at(camera_id).begin() + idx);
				frame_ids.erase(frame_ids.begin() + idx);
				LOG(INFO) << " frame_ids 1===== : " << frame_ids.size() << std::endl;

			}

			// LOG(INFO) << " frame_ids 1===== : " << frame_ids.size() << std::endl;
			// LOG(INFO) << " feat_ptr->map_camera_id_and_feature_.at(camera_id) 1===== : " << feat_ptr->map_camera_id_and_feature_.at(camera_id).size() << std::endl;

		}
	}
}

}